#include "testIntrinsicImage.h"
#include "cvxImage.h"
#include <iostream>
#include <time.h>
#include <vector>
#include <algorithm>
#include <list>
#include "nvmath.h"
#include "least_square.h"
#include "cvxQuadTree.h"
#include "cvxSf.h"
#include "cvxStroke.h"
#include "cvxIntrinsicSolver.h"
#include "cvxIo.h"
#include "cvxEMTreeSolver.h"
#include "cvxStereoIntrinsicSolver.h"
#include "cvxEdgeShadingRecovery.h"
#include "cvxDrawImage.h"
#include "cvxIntrinsicCluster.h"
#include "cv_segment.h"

using namespace std;
using namespace cv;
using cvx::Rgb16UImage;
using cvx::Rgb32FImage;
using cvx::Bw64FImage;
using cvx::Argb8UImage;
using cvx::Bw32FImage;
using cvx::Bw8UImage;
using cvx::Rgb8UImage;
using cvx::Yuv8UImage;
using nv::vec3f;



void TestIntrinsicImage::testLeastSquare()
{
	/*
	36 1  1.00
	37 1  0.9
	38 1  0.9
	39 1  0.81
	40 1  0.60
	41 1  0.56
	42 1  0.35  //answer -0.105   4.81
	*/	
	//ax + by = c


	vector<map<int, double>> matrix_vec;
	vector<double> right_vec;

	//left side
	std::map<int, double> iMap;
	iMap[0] = 36;	
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 37;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 38;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 39;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 40;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 41;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 42;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();

	
	//right side
	right_vec.push_back(1.0);
	right_vec.push_back(0.9);
	right_vec.push_back(0.9);
	right_vec.push_back(0.81);
	right_vec.push_back(0.6);
	right_vec.push_back(0.56);
	right_vec.push_back(0.35);

	assert(matrix_vec.size() == right_vec.size());

	vector<double> result(2);
	LeastSquare iSolver;
	bool isOk = iSolver.initialSolverLeft(matrix_vec, 2);
	if (isOk)
	{
		isOk = iSolver.solverRight(right_vec, result);		
	}
	if (isOk)
	{
		cout<<"first time = ";
		copy(result.begin(), result.end(), ostream_iterator<double>(cerr, " "));
		cout<<endl;

		//change the right vector
		for (int i =0 ; i<right_vec.size(); ++i)
		{
			right_vec[i] *= 2.0;
		}

		isOk = iSolver.solverRight(right_vec, result);
		cout<<"second time = ";
		copy(result.begin(), result.end(), ostream_iterator<double>(cerr, " "));
		cout<<endl;
	}	

}


void TestIntrinsicImage::testSCS_matrix()
{
	/*
	36 1  1.00
	37 1  0.9
	38 1  0.9
	39 1  0.81
	40 1  0.60
	41 1  0.56
	42 1  0.35  //answer is -0.105   4.81
	*/	

	spm_csc<double> A, At;
	std::vector<std::map<int, double>> matrix_vec;
	//create sparse matrix
	std::map<int, double> iMap;
	iMap[0] = 36;	
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
	iMap[0] = 37;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	iMap.clear();
 	iMap[0] = 38;
 	iMap[1] = 1;
 	matrix_vec.push_back(iMap);
 	iMap.clear();
// 	iMap[0] = 39;
// 	iMap[1] = 1;
// 	matrix_vec.push_back(iMap);
// 	iMap.clear();
// 	iMap[0] = 40;
// 	iMap[1] = 1;
// 	matrix_vec.push_back(iMap);
// 	iMap.clear();
// 	iMap[0] = 41;
// 	iMap[1] = 1;
// 	matrix_vec.push_back(iMap);
// 	iMap.clear();
// 	iMap[0] = 42;
// 	iMap[1] = 1;
// 	matrix_vec.push_back(iMap);
// 	iMap.clear();

	//convert matrix_vec to sparse matrix
	convert(matrix_vec, A, 2);
	LeastSquare::outPutMatrix(A, false);

	convertAT(A, At);
	LeastSquare::outPutMatrix(At, false);

}

int var_num = 0;
static void divideQuadTree(Mat &img)
{
	int w = img.cols;
	int h = img.rows;
	if (w == 1)
	{
		var_num++;
		return; 
	}
	cv::Rect r1 = cv::Rect(0, 0, w/2, h/2);
	cv::Rect r2 = cv::Rect(w/2, 0, w/2, h/2);
	cv::Rect r3 = cv::Rect(0, h/2, w/2, h/2);
	cv::Rect r4 = cv::Rect(w/2, h/2, w/2, h/2);

	Mat m1 = img(r1);
	Mat m2 = img(r2);
	Mat m3 = img(r3);
	Mat m4 = img(r4);
	Scalar mean, stdDev;

	int threshold = 2;
	cv::meanStdDev(m1, mean, stdDev);
	if (stdDev[0] > threshold || stdDev[1] > threshold || stdDev[2] > threshold)
	{
		divideQuadTree(m1);
	}
	else
	{
		var_num++;

	}

	cv::meanStdDev(m2, mean, stdDev);
	if (stdDev[0] > threshold || stdDev[1] > threshold || stdDev[2] > threshold)
	{
		divideQuadTree(m2);
	}
	else
	{
		var_num++;
	}

	cv::meanStdDev(m3, mean, stdDev);
	if (stdDev[0] > threshold || stdDev[1] > threshold || stdDev[2] > threshold)
	{
		divideQuadTree(m3);	
	}
	else
	{
		var_num++;
	}

	cv::meanStdDev(m4, mean, stdDev);
	if (stdDev[0] > threshold || stdDev[1] > threshold || stdDev[2] > threshold)
	{
		divideQuadTree(m4);
	}
	else
	{
		var_num++;		
	}


}

void TestIntrinsicImage::testQuadtree()
{
	Mat img =  imread("test_image\\synthetic_512_512.tif", 1);	 // synthetic_512_512.tif  table_512_512.png

	divideQuadTree(img);

	cout<<"var_num "<<var_num<<endl;
}



void TestIntrinsicImage::testDisparity()
{
	Mat leftImg = imread("test_image\\Bowling2\\view1.png", 1);
	Mat disMap = imread("test_image\\Bowling2\\disp1.png", 0);

	assert(!leftImg.empty());
	assert(!disMap.empty());

	int w = leftImg.cols;
	int h = leftImg.rows;
	Mat rightImg = cv::Mat::zeros(h, w, CV_8UC3);

	imshow("left", leftImg);
	imshow("disparity", disMap);

	Rgb8UImage l_img(&leftImg);
	Rgb8UImage r_img(&rightImg);
	Bw8UImage dis_map(&disMap);
	for (int y = 0; y<h; ++y)
	{
		for (int x = 0; x<w; ++x)
		{
			int sx = x - dis_map[y][x];
			if (sx>=0 && sx<w)
			{
				r_img[y][sx].r = l_img[y][x].r;
				r_img[y][sx].g = l_img[y][x].g;
				r_img[y][sx].b = l_img[y][x].b;
			}			
		}
	}

	imshow("right", rightImg);
}

void TestIntrinsicImage::testComputeDisparity()
{
	Mat leftImg = imread("test_image\\Bowling2\\view1.png", 0);
	Mat rightImg = imread("test_image\\Bowling2\\view5.png", 0);

	// 	Mat leftImg = imread("test_image\\table_left0.png", 0);
	// 	Mat rightImg = imread("test_image\\table_right0.png", 0);
	// 
	// 	imwrite("table_left.pgm", leftImg);
	// 	imwrite("table_right.pgm", rightImg);

	Mat gtDisparityLeft = imread("test_image\\Bowling2\\disp1.png", 0);
	Mat gtDisparityRight = imread("test_image\\Bowling2\\disp5.png", 0);

	int w = leftImg.cols;
	int h = leftImg.rows;

	Mat disMap = cv::Mat::zeros(h, w, CV_16S);

	enum { STEREO_BM=0, STEREO_SGBM=1, STEREO_HH=2, STEREO_VAR=3 };
	int alg = STEREO_SGBM;

	int numberOfDisparities = 32;
	StereoBM bm;
	bm.state->roi1 = cv::Rect(0, 0, w, h);
	bm.state->roi2 = cv::Rect(0, 0, w, h);
	bm.state->preFilterCap = 31;
	bm.state->SADWindowSize = 5;
	bm.state->minDisparity = 0;
	bm.state->numberOfDisparities = numberOfDisparities;
	bm.state->textureThreshold = 10;
	bm.state->uniquenessRatio = 15;
	bm.state->speckleWindowSize = 50;
	bm.state->speckleRange = 32;
	bm.state->disp12MaxDiff = 1;


	StereoSGBM sgbm;
	int cn = leftImg.channels();

	sgbm.SADWindowSize = 5;
	sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
	sgbm.minDisparity = 0;
	sgbm.numberOfDisparities = numberOfDisparities;
	sgbm.uniquenessRatio = 10;
	sgbm.speckleWindowSize = 100;
	sgbm.speckleRange = bm.state->speckleRange;
	sgbm.disp12MaxDiff = 1;
	sgbm.fullDP = alg == STEREO_HH;

	for (int i = 3; i<15; i+=2)
	{
		cout<<"window size = "<<i<<endl;
		sgbm.SADWindowSize = i;
		sgbm.P1 = 8*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
		sgbm.P2 = 32*cn*sgbm.SADWindowSize*sgbm.SADWindowSize;
		sgbm.minDisparity = 0;
		sgbm.numberOfDisparities = numberOfDisparities;
		sgbm.uniquenessRatio = 10;
		sgbm.speckleWindowSize = 100;
		sgbm.speckleRange = bm.state->speckleRange;
		sgbm.disp12MaxDiff = 1;
		sgbm.fullDP = alg == STEREO_HH;

		sgbm(leftImg, rightImg, disMap);
		disMap.convertTo(disMap, CV_8U, 255/(numberOfDisparities*16.));
		imshow("left", leftImg);
		imshow("right", rightImg);
		imshow("disparity", disMap);
		imshow("disparity ground truth", gtDisparityLeft);
		cvWaitKey(0);
	}

}



void TestIntrinsicImage::testGeneralQuadtreeImage()
{
//	Mat img =  imread("test_image\\table_left0.png", 1);
//	Mat img =  imread("test_image\\synthetic\\synthetic.tif", 1);
//	Mat img = imread("D:\\code\\IntrinsicRecovery\\target\\siggraph_2009\\baby.tif", 1);
	Mat img = imread("test_image\\gt_org\\synthetic.tif", 1); 
	//test_image\\gt_org\\synthetic.tif
	int w = img.cols;
	int h = img.rows;

	assert(!img.empty());
	unsigned int level = CvxQuadTree::GetLevel(std::max(w, h));
	CvxAdaptiveQuadTree *pTree = new CvxAdaptiveQuadTree(level, img, 5);

	pTree->create(NULL);
	pTree->balance();
 	pTree->draw(NULL);
// 
 	int leafNum = 0;
 	pTree->leafNumber(NULL, leafNum);
 	cout<<"leaf number = "<<leafNum<<endl;

	imshow("quad tree", pTree->m_orgimg);
	imwrite("quadtree.png", pTree->m_orgimg);

	
	while(1)
	{
		//random a position
		int x = rand()%w;
		int y = rand()%h;
		QtNode *node = pTree->LocateNode(x, y);
		//draw current node
		assert(node->isLeaf());
		int sz = 1<<node->_level;		
		cv::circle(pTree->m_orgimg, cv::Point(x, y), 2, Scalar(255, 0, 0, 0));
		cv::rectangle(pTree->m_orgimg, Rect(node->_xLocCode, node->_yLocCode, sz, sz), Scalar(0,255,0,0));


		//draw left node
		QtNode *n1, *n2;
		int num = pTree->topNeighbors(node, n1, n2);
		if (num == 2)
		{
			sz = 1<<n1->_level;		
			cv::rectangle(pTree->m_orgimg, Rect(n1->_xLocCode, n1->_yLocCode, sz, sz), Scalar(255,255,0,0));
			cv::rectangle(pTree->m_orgimg, Rect(n2->_xLocCode, n2->_yLocCode, sz, sz), Scalar(255,255,0,0));

		}
		else if (num == 1)
		{
			sz = 1<<n1->_level;		
			cv::rectangle(pTree->m_orgimg, Rect(n1->_xLocCode, n1->_yLocCode, sz, sz), Scalar(255,255,0,0));
		}		
		else
		{
			cout<<"x y = "<<x<<" "<<y<<endl;
			cout<<"does not found neighbors"<<endl;
		}


		imshow("quad tree in an image, show neighbor", pTree->m_orgimg);
		cv::circle(pTree->m_orgimg, cv::Point(x, y), 2, Scalar(255, 255, 255, 0));
		cvWaitKey(0);
		pTree->draw(NULL);
	}	
}

void TestIntrinsicImage::testCholmod()
{
	/*
	ax + cy = z 
	x  y  z
	36 1  1.00
	37 1  0.9
	38 1  0.9
	39 1  0.81
	40 1  0.60
	41 1  0.56
	42 1  0.35
	*/

	std::vector<std::map<int, double>> matrix_vec;
	std::vector<double> right_vec;
	
	std::map<int, double> iMap;
	iMap[0] = 36;	
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(1.0);
	iMap.clear();

	iMap[0] = 37;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.9);
	iMap.clear();

	iMap[0] = 38;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.9);
	iMap.clear();

	iMap[0] = 39;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.81);
	iMap.clear();

	iMap[0] = 40;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.60);
	iMap.clear();

	iMap[0] = 41;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.56);
	iMap.clear();

	iMap[0] = 42;
	iMap[1] = 1;
	matrix_vec.push_back(iMap);
	right_vec.push_back(0.35);
	iMap.clear();

	double result[2] = {0};
//	if (CvxLeastSquare::solver(matrix_vec, right_vec, true, 2, result))
	if(true)
	{
		cout<<"result = "<<result[0]<<" "<<result[1]<<endl;
		cout<<"should = "<<-0.105 <<" "<<4.81<<endl;
		//-0.105  4.81
	}
	else
	{
		cerr<<"error! test failed "<<endl;
	}		
	return ;
}

void TestIntrinsicImage::testMultipleScaleSample()
{
	Mat img =  imread("test_image\\doll\\doll_512.png", 1);
//	Mat img =  imread("test_image\\table_left0.png", 1);
	Mat showImg = imread("test_image\\doll\\doll_512_show.png", 1); //for test
	string xmlFile = string("path.xml");

	Mat pathMat;
	FileStorage fs(xmlFile, FileStorage::READ);
	fs["pathPts"]>>pathMat;
	fs.release();
//	cout<<pathMat<<endl;

	vector<cv::Point> pathPoint = pathMat;

	vector<cv::Rect> rectVec;
	CvxLocalSolver::adaptiveSample(pathPoint, 10, img, rectVec, 2);

	for (int i = 0; i<rectVec.size(); ++i)
	{
		cv::rectangle(showImg, rectVec[i], cv::Scalar(0, 0, 255));
	}
	imshow("candidate", showImg);
	imwrite("multiple_sample.png", showImg);
}

void TestIntrinsicImage::testCvxLocalSolver()
{
	Mat img =  imread("test_image\\doll\\doll_512.png", 1);
//	Mat img =  imread("test_image\\table_left0.png", 1);
	string xmlFile = string("path.xml");

	Mat pathMat;
	FileStorage fs(xmlFile, FileStorage::READ);
	fs["pathPts"]>>pathMat;
	fs.release();
	//	cout<<pathMat<<endl;

	vector<cv::Point> pathPoint = pathMat;

	CvxLocalSolver *pSolver = new CvxLocalSolver;

	pSolver->setImage(img);
	pSolver->computeEdgeMap(0.999f);
	pSolver->solveRect(pathPoint, 10, 5);

	Mat showShading = pSolver->m_shading.clone();
	showShading.convertTo(showShading, CV_8U, 255.0);

	imshow("shading", showShading);
}

void TestIntrinsicImage::testEMTreeSolver()
{
	Mat img =  imread("test_image\\synthetic\\synthetic.tif", 1);	
	Mat shadingMap = imread("test_image\\synthetic\\shading.png", 0);
	Mat shadingGT = imread("test_image\\synthetic\\synthetic_shading_truth.tif", 0);
	assert(!img.empty());
	assert(!shadingMap.empty());
	assert(!shadingGT.empty());

	int h = img.rows;
	int w = img.cols;

	shadingMap.convertTo(shadingMap, CV_32F, 1.0/255.0);

	Mat showShading;

	CvxEMtreeSolver solver;
	solver.setImage(img);
	solver.computeEdgeMap(0.999);

	solver.SetETree(shadingMap, 5);
	solver.SolveETree(true);

	printf("complete E tree solver\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("Etree shading", showShading);
//	imwrite("Etree_shading.png", showShading);

	
	solver.CalcMTree(true);
	printf("1\n");
	solver.SolveMtree(true);
	printf("2\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("Mtree shading", showShading);
//	imwrite("Mtree_shading.png", showShading);

	printf("complete M tree solver\n");

	solver.UpsampleAvector();
	printf("after up sample a vector\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("upsample shading", showShading);
//	imwrite("upsample_shading.png", showShading);
	imshow("upsample albedo", solver.m_albedo);
//	imwrite("upsample_albedo.png", solver.m_albedo);	
}

void TestIntrinsicImage::testLocalRefinement()
{
	Mat img =  imread("test_image\\refineBrush\\synthetic.tif", 1);
	Mat shadingMap = imread("test_image\\refineBrush\\shading.png", 0);

	assert(!img.empty());
	assert(!shadingMap.empty());

	string xmlFile = string("test_image\\refineBrush\\path.xml");

	Mat pathMat;
	FileStorage fs(xmlFile, FileStorage::READ);
	fs["pathPts"]>>pathMat;
	fs.release();

	vector<cv::Point> pathPts = pathMat;
	shadingMap.convertTo(shadingMap, CV_32F, 1.0/255.0);

	Mat showShading;

	CvxLocalRefineSolver refineSolver;
	refineSolver.setImage(img);
	refineSolver.computeEdgeMap(0.999);
	refineSolver.setInitShading(shadingMap);

 	showShading = refineSolver.m_shading.clone();
 	showShading.convertTo(showShading, CV_8U, 255.0);
 	imshow("before refinement", showShading);
 	imshow("before refinement albedo", refineSolver.m_albedo);

	refineSolver.solveRect(pathPts, 10);

 	showShading = refineSolver.m_shading.clone();
 	showShading.convertTo(showShading, CV_8U, 255.0);
 	imshow("after refinement", showShading);
 	imshow("after refinement albedo", refineSolver.m_albedo);
}


void TestIntrinsicImage::testAdaptiveQuadTree()
{
	vector<string> fileName;
	bool isOk = CvxIo::travel_dir("D:\\code\\IntrinsicRecovery\\target\\siggraph_2009\\*.tif", fileName);
	for(int i = 0; i < fileName.size(); i++)
	{
		 string name = "D:\\code\\IntrinsicRecovery\\target\\siggraph_2009\\" + fileName[i];
		 Mat img = cv::imread(name.c_str(), 1);
		 assert(!img.empty());

		 int w = img.rows;
		 int h = img.cols;

		 int level = CvxQuadTree::GetLevel(std::max(w, h));
		 CvxAdaptiveQuadTree *pTree = new CvxAdaptiveQuadTree(level, img, 10);

		 pTree->create(NULL);
		 pTree->balance();
		 pTree->draw(NULL);
		
		 imshow("adaptive quad", pTree->m_orgimg);

		 int leafNum = 0;
		 int pixelLeafNum = 0;
		 pTree->leafNumber(NULL, leafNum);
		 pTree->pixelLeafNumber(NULL, pixelLeafNum);
		 printf("leaf, pixel leaf, unknow number, ratio is %d %d %d %f\n", leafNum, pixelLeafNum, leafNum - pixelLeafNum, 1.0*(leafNum - pixelLeafNum)/(w*h));
		 cv::waitKey(2000);
	}
}

void TestIntrinsicImage::testAdaptiveInitialShading()
{
	string fileName = string("D:\\code\\IntrinsicRecovery\\target\\siggraph_2009\\pink_flower.tif");

	Mat image = imread(fileName, 1);
	assert(!image.empty());

	imshow("original image", image);
	int h = image.rows;
	int w = image.cols;
	int level = CvxQuadTree::GetLevel(std::max(h, w));

	Mat shadingMap = cv::Mat::zeros(h, w, CV_32F);

	CvxAdaptiveQuadTree tree = CvxAdaptiveQuadTree(level, image, 5);

	tree.create(NULL);
	tree.balance();
//	tree.adaptiveInitLeafData(image, NULL, 16);
//	tree.drawKnownShading(NULL, shadingMap);
}

void TestIntrinsicImage::testEMTreeWithAdaptiveInitialShading()
{
//	string fileName = string("test_image\\flower\\pink_flower.tif");
	string fileName = string("test_image\\synthetic_org\\synthetic.tif");
	Mat img =  imread(fileName, 1);	
	assert(!img.empty());

	int h = img.rows;
	int w = img.cols;

	Mat showShading;
	Mat grayImage;
	cv::cvtColor(img, grayImage, CV_BGR2GRAY);

	CvxEMtreeSolver solver;
	solver.setImage(img);

	solver.SetETree(5, 16);
	solver.SolveETree(true);

	imshow("original image", img);
	imshow("gray image", grayImage);

	printf("complete E tree solver\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("Etree shading", showShading);

		
	solver.CalcMTree(true);
	solver.SolveMtree(true);
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("Mtree shading", showShading);

	printf("complete M tree solver\n");

	solver.UpsampleAvector();
	printf("after up sample a vector\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("upsample shading", showShading);
	imshow("upsample albedo", solver.m_albedo);
}

void TestIntrinsicImage::testStereoSolverIndex()
{
	
	Mat leftImage = cv::imread("test_image\\turtle_image\\left\\img_0001.png", 1);
	Mat rightImage = cv::imread("test_image\\turtle_image\\right\\img_0001.png", 1);

	
 //   Mat leftImage = cv::imread("org_image\\butter_fly\\butter_fly_left_small.bmp", 1);
//	Mat rightImage = cv::imread("org_image\\butter_fly\\butter_fly_right_small.bmp", 1);

	assert(!leftImage.empty());
	assert(!rightImage.empty());

	float threshold = 0.999;
	int stddevThreshold = 5;
	int sizeThreshold = 16;

	//set E tree
	CvxStereoIntrinsicSolver solver;
	solver.setImage(leftImage, rightImage, threshold);
	solver.SetETree(stddevThreshold, sizeThreshold);
	solver.MatchLeftRightETree(2.0, 4, true);
	solver.SolveETree(true, false, true);

	solver.CalcMTree(true);
	solver.SolveMtree(true, true);
	solver.UpsampleAvector(true);
}

void TestIntrinsicImage::visualizeEMTree()
{
	//Mat leftImage = cv::imread("E:\\code\\IntrinsicImage\\IntrinsicImage\\IntrinsicImage\\org_image\\girl.png", 1);
	Mat leftImage = cv::imread("test_image\\turtle_image\\left\\img_0001.png", 1);

	assert(!leftImage.empty());

	Mat grayImage = leftImage.clone();
	cv::cvtColor(grayImage, grayImage, CV_BGR2GRAY);
	cv::imshow("gray image", grayImage);

	float edgeThreshold = 0.999;
	int stddevThreshold = 5;
	int sizeThreshold = 256;

	Mat showShading;

	CvxEMtreeSolver solver;

	solver.setImage(leftImage);
	solver.computeEdgeMap(edgeThreshold);
	solver.saveEdgeMap("edge.png");

	solver.SetETree(stddevThreshold, sizeThreshold);
	solver.SaveQuadTree("quad.png", true);
	solver.SaveColorQuadTree("emtree_node.png", true);
	solver.SolveETree(true);


	printf("complete E tree solver\n");
//	imwrite("Etree_shading.png", showShading);

	

	solver.CalcMTree(true);
	printf("1\n");
	solver.SolveMtree(true);
	printf("2\n");
//	showShading = solver.m_shading.clone();				
//	showShading.convertTo(showShading, CV_8U, 255.0);
//	imshow("Mtree shading", showShading);
//	imwrite("Mtree_shading.png", showShading);

	solver.SaveShadingImage("roughShading.png", true);

	printf("complete M tree solver\n");

	solver.UpsampleAvector();
	printf("after up sample a vector\n");
	solver.SaveShadingImage("final_shading.png", true);
	solver.SaveAlbedoImage("final_albedo.png", true);

}

void TestIntrinsicImage::testMatchAdaptiveTree()
{
	CvxStereoIntrinsicSolver solver;
	Mat leftImage = cv::imread("test_image\\turtle_image\\left\\img_0001.png", 1);
	Mat rightImage = cv::imread("test_image\\turtle_image\\right\\img_0001.png", 1);

	assert(!leftImage.empty());
	assert(!rightImage.empty());

	float threshold = 0.999;
	int stddevThreshold = 5;
	int sizeThreshold = 16;

	//set E tree
	solver.setImage(leftImage, rightImage, threshold);
	solver.SetETree(stddevThreshold, sizeThreshold);

	solver.MatchLeftRightETree(2.0, 4);
}

void TestIntrinsicImage::reColorTurtle()
{
	Mat leftShading = cv::imread("test_image\\turtle_image\\shading_result\\left_shading.png", 0);
	Mat rightShading = cv::imread("test_image\\turtle_image\\shading_result\\right_shading.png", 0);
	assert(!leftShading.empty());
	assert(!rightShading.empty());

	Mat leftColorImage = cv::imread("test_image\\turtle_image\\left\\color_0001.png", 1);
	Mat rightColorImage = cv::imread("test_image\\turtle_image\\right\\color_0001.png", 1);
	assert(!leftColorImage.empty());
	assert(!rightColorImage.empty());

	cv::imshow("org left color image", leftColorImage);

	int h = leftColorImage.rows;
	int w = leftColorImage.cols;
	Mat leftMask  = cv::Mat::zeros(h, w, CV_8UC1);
	Mat rightMask = cv::Mat::zeros(h, w, CV_8UC1);

	Rgb8UImage left_color_img(&leftColorImage);
	Rgb8UImage right_color_img(&rightColorImage);

	Bw8UImage left_mask(&leftMask);
	Bw8UImage right_mask(&rightMask);

	// get the color of center image
	int cx = w/2;
	int cy = h/2;
	int cr = left_color_img[cy][cx].r;
	int cg = left_color_img[cy][cx].g;
	int cb = left_color_img[cy][cx].b;
	int threshold = 65;

	// get the mask of the left image and right image
	for(int y = 0; y<h; y++)
	{
		for(int x =0; x<w; x++)
		{
			if(abs(left_color_img[y][x].r - cr) <= threshold &&
			   abs(left_color_img[y][x].g - cg) <= threshold &&
			   abs(left_color_img[y][x].b - cb) <= threshold )
			{
				left_color_img[y][x].r = 0;
				left_mask[y][x] = 255;
			}

			if(abs(right_color_img[y][x].r - cr) <= threshold &&
			   abs(right_color_img[y][x].g - cg) <= threshold &&
			   abs(right_color_img[y][x].b - cb) <= threshold )
			{
				right_mask[y][x] = 255;
			}
			
		}
	}
	cv::imshow("recolor", leftColorImage);
	cv::imshow("left mask", leftMask);
	cv::imshow("right mask", rightMask);

	//change reflectance of the original image
	Mat leftImage = cv::imread("test_image\\turtle_image\\left\\img_0001.png", 1);
	Mat rightImage = cv::imread("test_image\\turtle_image\\right\\img_0001.png", 1);
	assert(!leftImage.empty());
	assert(!rightImage.empty());

	cv::imshow("left", leftImage);
	cv::imshow("right", rightImage);

	Rgb8UImage left_img(&leftImage);
	Rgb8UImage right_img(&rightImage);
	Bw8UImage left_sha(&leftShading);
	Bw8UImage right_sha(&rightShading);
	

	Mat leftSpecular = cv::imread("test_image\\turtle_image\\left\\specular_0001.png", 1);
	Mat rightSpecular = cv::imread("test_image\\turtle_image\\right\\specular_0001.png", 1);
	assert(!leftSpecular.empty());
	assert(!rightSpecular.empty());
	
	Rgb8UImage left_spe(&leftSpecular);
	Rgb8UImage right_spe(&rightSpecular);



	// new color
	int nr = 0.379 * 255 * 1.0;
	int ng = 0.785 * 255 * 1.0;
	int nb = 0.278 * 255 * 1.0;

	bool addSpecular = false;

	for(int y = 0; y<h; y++)
	{
		for(int x = 0; x<w; x++)
		{
			if(left_mask[y][x] == 255)
			{
				float s = left_sha[y][x]/255.0;
				if(addSpecular)
				{
					left_img[y][x].r = nr * s + left_spe[y][x].r;
					left_img[y][x].g = ng * s + left_spe[y][x].g;
					left_img[y][x].b = nb * s + left_spe[y][x].b;	
				}
				else
				{
					left_img[y][x].r = nr * s;
					left_img[y][x].g = ng * s;
					left_img[y][x].b = nb * s;	

				}

			}
			if(right_mask[y][x] == 255)
			{
				float s = right_sha[y][x]/255.0;
				if(addSpecular)
				{
					right_img[y][x].r = nr * s + right_spe[y][x].r;
					right_img[y][x].g = ng * s + right_spe[y][x].g;
					right_img[y][x].b = nb * s + right_spe[y][x].b;
				}
				else
				{
					right_img[y][x].r = nr * s;
					right_img[y][x].g = ng * s;
					right_img[y][x].b = nb * s;
				}
						
			}
		}
	}
	cv::imshow("new left", leftImage);
	cv::imshow("new right", rightImage);

	cv::imwrite("reColorTurtleLeft.png", leftImage);
	cv::imwrite("reColorTurtleRight.png", rightImage);
}

void TestIntrinsicImage::testIterativeSolver()
{
	float edgeThreshold = 0.999;
	int sizeThreshold = 256;
	int stddevThreshold = 5;
	float minimumShadingScale = 1.2;

	//Mat img = cv::imread("test_image\\turtle_image\\mid\\img_0001.png", 1);
	//Mat img = cv::imread("test_image\\synthetic_org\\synthetic.tif", 1);
	//Mat img = cv::imread("test_image\\flower\\pink_flower.tif", 1);
	//Mat img = cv::imread("org_image\\girl.png", 1);
	//Mat img = cv::imread("org_image\\baby.tif", 1);
	Mat img = cv::imread("org_image\\deer.png", 1);
	assert(!img.empty());

	Mat grayImg = img.clone();
	cv::cvtColor(grayImg, grayImg, CV_BGR2GRAY);
	cv::imshow("gray image", grayImg);

	Mat curReflectance = img.clone();

	int num = 5;
	for(int i = 0; i<num; i++)
	{
		CvxEMtreeSolver solver;

		solver.setImage(curReflectance);
		solver.computeEdgeMap(edgeThreshold);
		
		solver.SetETree(stddevThreshold, sizeThreshold, minimumShadingScale);
		solver.SolveETree(true);
		
		printf("complete E tree solver\n");
			
		solver.CalcMTree(true);
		solver.SolveMtree(false);

		printf("complete M tree solver\n");

		solver.UpsampleAvector();
		

		//solve again
//		solver.UpdataETreeEdgeNode();
//		solver.SolveETree(true);
//		solver.CalcMTree(true);
//		solver.SolveMtree(true);
//		solver.UpsampleAvector();

//		printf("solve again finished.\n");

		imshow("albedo", solver.m_albedo);
		printf("after up sample a vector\n");

		//get new shading
		Mat showShading = grayImg.clone();

		Rgb8UImage albedo_img(&(solver.m_albedo));
		Bw8UImage sha_img(&showShading);
		Rgb8UImage org_img(&img);

		int h = grayImg.rows;
		int w = grayImg.cols;
		for(int y = 0; y<h; y++)
		{
			for(int x = 0; x<w; x++)
			{
				float sr = 1.0 * org_img[y][x].r/albedo_img[y][x].r;
				float sg = 1.0 * org_img[y][x].g/albedo_img[y][x].g;
				float sb = 1.0 * org_img[y][x].b/albedo_img[y][x].b;
				sha_img[y][x] = std::min(255.0, (sr + sg + sb)/3.0 * 255);
			}
		}

		cv::imshow("albedo", solver.m_albedo);
		cv::imshow("shading", showShading);
		cv::imwrite("itereatedShading.png", showShading);
		cv::waitKey(0);

		curReflectance = solver.m_albedo.clone();
	}
}

void TestIntrinsicImage::testEdgeAssumption()
{
	Mat orgImage  = cv::imread("org_image\\sun\\sun_original.png", 1);
	Mat maskImage = cv::imread("org_image\\sun\\sun_mask.png", 0);
	Mat shadingGD = cv::imread("org_image\\sun\\sun_shading.png", 0);
	Mat grayImage = orgImage.clone();
	cv::cvtColor(grayImage, grayImage, CV_BGR2GRAY);


	CvxEMtreeSolver solver;
	solver.setImage(orgImage);
	solver.computeEdgeMap(0.99);

	Mat edgeMap = solver.m_edgeMap.clone();
	vector<float> ratioVec;
	
	cv::imshow("edge map", edgeMap);
	cv::imshow("mask map", maskImage);

	int h = orgImage.rows;
	int w = orgImage.cols;

	//gray to minimum value
	grayImage = solver.m_miniShading.clone();
	grayImage.convertTo(grayImage, CV_8UC1, 255.0);


	Bw8UImage mask_img(&maskImage);
	Bw8UImage sha_img(&shadingGD);
	Bw8UImage edge_img(&edgeMap);
	Bw8UImage gray_img(&grayImage);

	Rgb8UImage org_img(&orgImage);

	for(int y = 0; y<h; y++)
	{
		for(int x = 0; x<w; x++)
		{
			if(mask_img[y][x] != 0)
			{
				float ratio = 1.0 * sha_img[y][x]/gray_img[y][x];
				ratioVec.push_back(ratio);
				if(ratio < 1.0)
				{
					org_img[y][x].r = 255;
				}
				else if(ratio > 1.0 && ratio < 2.0)
				{
					org_img[y][x].g = 255;
				}
				else if(ratio > 2.0)
				{
					org_img[y][x].b = 255;
				}
			}
			else
			{
				edge_img[y][x] = 0;
			}
		}
	}

	std::sort(ratioVec.begin(), ratioVec.end());

	int test = 0;
	cv::imshow("differ edge", orgImage);
}

void TestIntrinsicImage::testGiveEdgeNodeGroundTruth()
{
	Mat img  = cv::imread("org_image\\sun\\sun_original.png", 1);
	Mat maskImage = cv::imread("org_image\\sun\\sun_mask.png", 0);
	Mat shadingGD = cv::imread("org_image\\sun\\sun_shading.png", 0);

	int stddevThreshold = 5;
	int sizeThrehold  = 256;
	float mininumShadingScale = 1.0;

	CvxEMtreeSolver solver;
	solver.setImage(img);
	solver.computeEdgeMap(0.99);

	imshow("original image", img);
	imshow("edge map", solver.m_edgeMap);


	double tt = clock();

	solver.SetETree(stddevThreshold, sizeThrehold, mininumShadingScale);

	solver.SetGroundTruth2EdgeNode(shadingGD);

	solver.SolveETree(true);	
	
	printf("complete E tree solver\n");
	Mat showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
//	imshow("Etree shading", showShading);

		
	solver.CalcMTree(true);
	solver.SolveMtree(true);

	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
//	imshow("Mtree shading", showShading);

	printf("complete M tree solver\n");

	double tUpSample = clock();
	solver.UpsampleAvector();
	printf("upSample cost time %f ms\n", clock() - tUpSample);

	printf("cost time %f ms\n",clock() - tt);

	printf("after up sample a vector\n");
	showShading = solver.m_shading.clone();				
	showShading.convertTo(showShading, CV_8U, 255.0);
	imshow("upsample shading", showShading);
	imshow("upsample albedo", solver.m_albedo);
}

void TestIntrinsicImage::testEdgeSolver()
{
	Mat img       = cv::imread("org_image\\sun\\sun_original.png", 1);
	Mat maskImage = cv::imread("org_image\\sun\\sun_mask.png", 0);
	Mat shadingGD = cv::imread("org_image\\sun\\sun_shading.png", 0);
	
    CvxEMtreeSolver solver;
	solver.setImage(img);
	//solver.computeEdgeMap(0.99);

	//imshow("original image", img);
	//imshow("edge map", solver.m_edgeMap);

	Mat edgeMap = solver.m_edgeMap.clone();

	//mask out fake edges
	Bw8UImage mask_img(&maskImage);
	Bw8UImage edge_map(&edgeMap);
	int h = img.rows;
	int w = img.cols;
	for(int y = 0; y<h; y++)
	{
		for(int x =0; x<w; x++)
		{
			if(mask_img[y][x] == 0)
			{
				edge_map[y][x] = 0;
			}
		}
	}

	cv::imshow("edge map", edgeMap);

	//show
	Mat edgeShadingGD = shadingGD.clone();

	Bw8UImage edge_sha_gd(&edgeShadingGD);
	for(int y = 0; y<h; y++)
	{
		for(int x =0; x<w; x++)
		{
			if(edge_map[y][x] == 0)
			{
				edge_sha_gd[y][x] = 0;
			}
		}
	}

	cv::imshow("edge shading ground truth", edgeShadingGD);

	//show edge albode
	Mat edgeAlbedoGD = img.clone();

	Rgb8UImage edge_albedo_gd(&edgeAlbedoGD);
	for(int y = 0; y<h; y++)
	{
		for(int x =0; x<w; x++)
		{
			if(edge_map[y][x] != 0)
			{
				double s = edge_sha_gd[y][x]/255.0;
				edge_albedo_gd[y][x].r = min(255.0, edge_albedo_gd[y][x].r/s);
				edge_albedo_gd[y][x].g = min(255.0, edge_albedo_gd[y][x].g/s);
				edge_albedo_gd[y][x].b = min(255.0, edge_albedo_gd[y][x].b/s);
			}
			else
			{
				edge_albedo_gd[y][x].r = 0.0;
				edge_albedo_gd[y][x].g = 0.0;
				edge_albedo_gd[y][x].b = 0.0;
			}
		}
	}
	cv::imshow("edge albedo ground truth", edgeAlbedoGD);
	

	CvxEdgeShadingRecovery edgeRecovery;
	edgeRecovery.setImage(img);
	edgeRecovery.calculateWeight(true);
//	edgeRecovery.testWeightWithGroundTruth(edgeAlbedoGD);
}

void TestIntrinsicImage::testRecolorBird()
{
	/*
	Mat orgImage = cv::imread("test_image\\\love_bird\\left0_corner.png", 1);
	Mat refImage = cv::imread("test_image\\\love_bird\\left0_corner_albedo.png", 1);
	Mat shaImage = cv::imread("test_image\\\love_bird\\left0_corner_shading.png", 0);
	Mat maskImage = cv::imread("test_image\\\love_bird\\left_mask_3.png", 0);
	*/


	Mat orgImage = cv::imread("test_image\\\love_bird\\right0_corner.png", 1);
	Mat refImage = cv::imread("test_image\\\love_bird\\right0_corner_albedo.png", 1);
	Mat shaImage = cv::imread("test_image\\\love_bird\\right0_corner_shading.png", 0);
	Mat maskImage = cv::imread("test_image\\\love_bird\\right_mask_3.png", 0);


	assert(!orgImage.empty());
	assert(!refImage.empty());
	assert(!shaImage.empty());
	assert(!maskImage.empty());

	cv::imshow("org image", orgImage);
	//change hsv color
	Mat hsvImage = orgImage.clone();
	cv::cvtColor(hsvImage, hsvImage, CV_BGR2HSV, 1);

//	cv::imshow("hsv image", hsvImage);


	// change ref challeels
	Bw8UImage mask_img(&maskImage);
	int h = orgImage.rows;
	int w = orgImage.cols;

	Rgb8UImage ref_map(&refImage);
	Bw8UImage  sha_map(&shaImage);
	for(int y = 0; y<h; y++)
	{
		for(int x = 0; x<w; x++)
		{
			if(mask_img[y][x] != 0)
			{
				int r = ref_map[y][x].r;
				int g = ref_map[y][x].g;
				int b = ref_map[y][x].b;

				ref_map[y][x].r = b;
				ref_map[y][x].b = r;
			}
		}
	}

	cv::imshow("new reflectance", refImage);


	//re generate image
	Mat reColorImage = orgImage.clone();
	Rgb8UImage re_color_img(&reColorImage);
	for(int y = 0; y<h; y++)
	{
		for(int x = 0; x<w; x++)
		{
			if(mask_img[y][x] != 0)
			{
				float s = sha_map[y][x]/255.0;
				re_color_img[y][x].r = ref_map[y][x].r * s;
				re_color_img[y][x].g = ref_map[y][x].g * s;
				re_color_img[y][x].b = ref_map[y][x].b * s;
			}
		}
	}

	cv::imshow("re colored image", reColorImage);
	cv::imwrite("recolored.png", reColorImage);

}

void TestIntrinsicImage::testEMTreeSpeed()
{
	float edgeThreshold = 0.999;
	int sizeThreshold = 256;
	int stddevThreshold = 5;
	float minimumShadingScale = 1.0;

	//Mat img = cv::imread("test_image\\synthetic_org\\synthetic.tif", 1);
	//Mat img = cv::imread("org_image\\moscow_small.png", 1); //pine_apple_left.png
	Mat img = cv::imread("org_image\\pine_apple_left.png", 1);
	
	assert(!img.empty());


	CvxEMtreeSolver solver;

	solver.setImage(img);
	double time = clock();
	solver.computeEdgeMap(edgeThreshold);	
	solver.SetETree(stddevThreshold, sizeThreshold, minimumShadingScale);
	solver.SolveETree(true);
	solver.CalcMTree(true);
	solver.SolveMtree(true);
	solver.UpsampleAvector();
	printf("cost time %f\n", clock() - time);

}

void TestIntrinsicImage::testIterativeEdgeSolver()
{
	Mat img = cv::imread("test_image\\girl.bmp", 1);
	assert(!img.empty());

	CvxEdgeShadingRecovery edgeRecovery;
	edgeRecovery.setImage(img);
	edgeRecovery.calculateWeight(true);

	int iterNum  = 10;
	float lambda = 0.5;
	edgeRecovery.iterativeSolve(iterNum, lambda);
}

void TestIntrinsicImage::clusterSingleImage()
{
	Mat image = cv::imread("test_image\\turtule\\left\\img_0001.png", 1);
//	Mat image = cv::imread("test_image\\fruits.jpg", 1);
	Mat maskImage = cv::imread("test_image\\turtule\\left\\alpha_0001.png", 0);
	Mat depthImage = cv::imread("test_image\\turtule\\left\\depth_0001.png", 0);

	assert(!image.empty());
	assert(!maskImage.empty());
	assert(!depthImage.empty());

	cv::imshow("org image", image);
//	cv::imshow("depth image", depthImage);

	depthImage.convertTo(depthImage, CV_32FC1, 1.0/255.0);

	Mat label;
	CvxIntrinsicCluster::clusterSingleImage(image, depthImage, maskImage, label, true);
}

void TestIntrinsicImage::testQuadTreeCluster()
{
	char *inputName = "test_image\\synthetic\\synthetic.tif";   //
	Mat image = cv::imread(inputName, 1);

	cv::imshow("original image", image);

	int h = image.rows;
	int w = image.cols;

	Mat labelImage;
	int labelNum = 0;
	float sigma = 0.5;
	float kk = 500;
	int minSize = 400;
	//cluster
	bool isok = CvSegment::segment(image, labelImage, labelNum, sigma,  kk ,  minSize);
	assert(isok);

	unsigned int level = CvxQuadTree::GetLevel(std::max(w, h));
	CvxAdaptiveQuadTree *pTree = new CvxAdaptiveQuadTree(level, image, 5);
	assert(pTree);

	pTree->create(NULL);
	pTree->balance();

	//initial data in the leaf

	pTree->draw(NULL);

	cv::imshow("quad tree", pTree->m_orgBackUp);

	//get edge map
	Mat edgeMap;
	CvxIntrinsicSolver solver;
	solver.setImage(image);
	solver.computeEdgeMap(0.999);

	pTree->adaptiveInitLeafData(image, solver.m_edgeMap, NULL, 256, 1.0);

	
	pTree->clusterLargeNode(NULL, 16, labelImage, labelNum);
 //	return;

	

	vector<Scalar> colors;
	pTree->drawClusteredNode(NULL, labelNum, colors);

	cv::imshow("clustered quad tree", pTree->m_orgBackUp);

}

